#include "bsp/bsp.h"
#include "gpio.h"

#define N_EXTI 16

struct subscription_t {
    GPIO_TypeDef* port;
    void (*callback)(void);
    void* ctx;
} subscriptions[N_EXTI];

static u8 clk_enabled_[PORT_MAX] = {0};

static __INLINE u16 get_pin_number(u16 pin_mask_) {
	uint16_t pin_number = 0;
	uint16_t pin_mask = pin_mask_ >> 1;
	while (pin_mask) {
		pin_mask >>= 1;
		pin_number++;
	}
	return pin_number;
}

static inline IRQn_Type get_irq_number(u16 pin_number) {
    switch (pin_number) {
        case 0: return EXTI0_IRQn;
        case 1: return EXTI1_IRQn;
        case 2: return EXTI2_IRQn;
        case 3: return EXTI3_IRQn;
        case 4: return EXTI4_IRQn;
        case 5:
        case 6:
        case 7:
        case 8:
        case 9: return EXTI9_5_IRQn;
        case 10:
        case 11:
        case 12:
        case 13:
        case 14:
        case 15: return EXTI15_10_IRQn;
        default: return (IRQn_Type)0; // impossible
    }
}


static inline GPIO_TypeDef* get_gpio_port(uint32_t io) {
	u16 io_port = PORT(io);
    switch (io_port) {
        case 0: return GPIOA;
        case 1: return GPIOB;
        case 2: return GPIOC;
        case 3: return GPIOD;
        case 4: return GPIOE;
        case 5: return GPIOF;
        case 6: return GPIOG;
        default: return (GPIO_TypeDef*)0; // impossible
    }
}

static void _gpio_enable_clk(u32 io) {
  	GPIO_TypeDef *port = get_gpio_port(io);
	if (port == NULL) {
		return;
	}
    if (port == GPIOA) {
        __HAL_RCC_GPIOA_CLK_ENABLE();
		clk_enabled_[0] = 1;
    } else if (port == GPIOB) {
        __HAL_RCC_GPIOB_CLK_ENABLE();
		clk_enabled_[1] = 1;
    } else if (port == GPIOC) {
        __HAL_RCC_GPIOC_CLK_ENABLE();
		clk_enabled_[2] = 1;
    } else if (port == GPIOD) {
        __HAL_RCC_GPIOD_CLK_ENABLE();
		clk_enabled_[3] = 1;
    } else if (port == GPIOE) {
        __HAL_RCC_GPIOE_CLK_ENABLE();
		clk_enabled_[4] = 1;
    } else if (port == GPIOF) {
        __HAL_RCC_GPIOF_CLK_ENABLE();
		clk_enabled_[5] = 1;
    } else if (port == GPIOG) {
        __HAL_RCC_GPIOG_CLK_ENABLE();
		clk_enabled_[6] = 1;
    }
#ifdef GPIOH
	else if (port == GPIOH) {
        __HAL_RCC_GPIOH_CLK_ENABLE();
		clk_enabled_[7] = 1;
    } 
#endif
	else {
        return;
    }
}

void _gpio_init(u32 io, u32 mode, u32 pull, u32 speed, u32 alt) {
  	GPIO_InitTypeDef   GPIO_InitStructure;
  	GPIO_TypeDef *port = get_gpio_port(io);
	if (port == NULL) {
		return;
	}
	_gpio_enable_clk(io);
  	GPIO_InitStructure.Mode = mode;
  	GPIO_InitStructure.Pull = pull;
	GPIO_InitStructure.Speed = speed;
    GPIO_InitStructure.Alternate = alt;
  	GPIO_InitStructure.Pin = PIN(io);
  	HAL_GPIO_Init(port, &GPIO_InitStructure);
}


void gpio_init(u32 io, u32 mode, u32 pull, u32 speed) {
    _gpio_init(io, mode, pull, speed, 0);
}

void gpio_analog_init(u32 io) {
    gpio_init(io, GPIO_MODE_ANALOG, GPIO_NOPULL, 0);
}

void gpio2_init_input_pull_none(u32 io) {
	gpio_init(io, GPIO_MODE_INPUT, GPIO_NOPULL, GPIO_SPEED_MEDIUM);
}


void gpio_af_init(u32 io, u32 mode, u32 pull, u32 speed, u32 alt) {
    _gpio_init(io, mode, pull, speed, alt);
}

void gpio_deinit(u32 io) {
  	GPIO_TypeDef *port = get_gpio_port(io);
	if (port == NULL) {
		return;
	}
  	HAL_GPIO_DeInit(port, PIN(io));
}

void gpio_write(u32 io, bool set) {
	if (io != INVALD_PIO) {
		if (clk_enabled_[PORT(io)] == 0) {
			_gpio_enable_clk(io);
		}
		HAL_GPIO_WritePin(get_gpio_port(io), PIN(io), set ? GPIO_PIN_SET : GPIO_PIN_RESET);
	}
}

bool gpio_read(u32 io) {
	if (io != INVALD_PIO) {
		GPIO_TypeDef * port = get_gpio_port(io);
		return port && (port->IDR & PIN(io));
	}
	return false;
}

bool gpio_get_output(u32 io) {
	if (io == INVALD_PIO) {
		return false;
	}
	GPIO_TypeDef * port = get_gpio_port(io);
	return port && (port->ODR & PIN(io));
}

void exti_subscribe(u32 io, u32 edge, exti_handler handler) {
	u32 mode = MODE_INPUT | EXTI_IT;
	if (edge & EXTI_EDGE_FALL) {
		mode |= TRIGGER_FALLING;
	}
	if (edge & EXTI_EDGE_RAISE) {
		mode |= TRIGGER_RISING;
	}

	gpio_init(io, mode, GPIO_NOPULL, 0);

	u16 pin_number = get_pin_number(PIN(io));
	IRQn_Type n_irq = get_irq_number(pin_number);
	if (n_irq == 0 || pin_number >= N_EXTI) {
		return;
	}
	__disable_irq();
	subscriptions[pin_number].callback = handler;
	subscriptions[pin_number].ctx = NULL;
	subscriptions[pin_number].port = NULL;
	__enable_irq();

	__HAL_GPIO_EXTI_CLEAR_IT(PIN(io));
	
	HAL_NVIC_EnableIRQ(n_irq);
}
void exti_unsubscribe(u32 io) {
  	GPIO_TypeDef *port = get_gpio_port(io);
	if (port == NULL) {
		return;
	}
  	HAL_GPIO_DeInit(port, PIN(io));
	__HAL_GPIO_EXTI_CLEAR_IT(PIN(io));
	uint16_t pin_number = get_pin_number(PIN(io));
	if (pin_number >= N_EXTI) {
		return;
	}
	subscriptions[pin_number].callback = NULL;
	subscriptions[pin_number].ctx = NULL;
	subscriptions[pin_number].port = NULL;
}

static __INLINE void exti_handle(u16 exti_number) {
    if(__HAL_GPIO_EXTI_GET_IT(1 << exti_number) == RESET) {
        return; // This interrupt source did not trigger the interrupt line
    }

    __HAL_GPIO_EXTI_CLEAR_IT(1 << exti_number);

    struct subscription_t *subscription = &subscriptions[exti_number];
    if (subscription->callback) {
        (subscription->callback)();
    }
}

bool exti_is_pending_clear(u32 io) {
	if (__HAL_GPIO_EXTI_GET_IT(PIN(io)) == SET){
		__HAL_GPIO_EXTI_CLEAR_IT(PIN(io));
		return true;
	}
	return false;
}


/** @brief Entrypoint for the EXTI line 0 interrupt. */
void EXTI0_IRQHandler(void) {
    exti_handle(0);
}

/** @brief Entrypoint for the EXTI line 1 interrupt. */
void EXTI1_IRQHandler(void) {
    exti_handle(1);
}

/** @brief Entrypoint for the EXTI line 2 interrupt. */
void EXTI2_IRQHandler(void) {
    exti_handle(2);
}

/** @brief Entrypoint for the EXTI line 3 interrupt. */
void EXTI3_IRQHandler(void) {
    exti_handle(3);
}

/** @brief Entrypoint for the EXTI line 4 interrupt. */
void EXTI4_IRQHandler(void) {
    exti_handle(4);
}

/** @brief Entrypoint for the EXTI lines 5-9 interrupt. */
void EXTI9_5_IRQHandler(void) {
    exti_handle(5);
    exti_handle(6);
    exti_handle(7);
    exti_handle(8);
    exti_handle(9);
}

/** @brief This function handles EXTI lines 10-15 interrupt. */
void EXTI15_10_IRQHandler(void) {
    exti_handle(10);
    exti_handle(11);
    exti_handle(12);
    exti_handle(13);
    exti_handle(14);
    exti_handle(15);
}

